
#include "lpc21xx.h"
#include "flash_lpc21xx.h"
#include <errno.h>
#include <FreeRTOS.h>

struct iap_in {
  unsigned int cmd;
  unsigned int par[4];
};

// CCLK definitions BOARD_CONFIG_FOSC_MHZ
#define CCLK			(configCPU_CLOCK_MHZ * 1000) 

typedef void (*IAP)(struct iap_in *in, unsigned int *result);

#define iap_entry ((IAP) 0x7FFFFFF1)       // IAP Entry Point



//VICDefVectAddr = (unsigned int) def_isr; // for Spurious Interrupts

static unsigned char sect_value[ FLASH_LPC21XX_MAX_BUFFER_SIZE ]; 

int flash_lpc21xx_has_sign()
{
//	unsigned char *sign_val;
	uint32 *flash_addr;

    flash_addr = (uint32 *) FLASH_LPC21XX_SIGN_WORD_ADDR;
	//*(sign_val) = (unsigned char) *(flash_addr);
	if (*(flash_addr) == FLASH_LPC21XX_SIGN_WORD_VALUE)
	{
		return FLASH_LPC_SIGN_WORD_PRESENT;
	}
	else
	{
		return -EINVAL;
	}
}

unsigned int flash_lpc21xx_init()
{
    int i;
	unsigned char *flash_addr;

	for (i=0;i<FLASH_LPC21XX_MAX_BUFFER_SIZE;i++)
	{
	    flash_addr = (unsigned char*) (FLASH_LPC21XX_DATA_SECTOR_ADDR + i );
		sect_value[i] = (unsigned char) *(flash_addr);
	}
	return FLASH_LPC21XX_INIT_DONE;
}

unsigned int flash_lpc21xx_write_sign_word()
{
	uint32 sign_word;
	sign_word = FLASH_LPC21XX_SIGN_WORD_VALUE;
	
	flash_lpc21xx_write_block (0, (unsigned char *)&sign_word, 4);
	return SIGN_WORD_WRITE_DONE;
}
 
	

//Note: THIS FUNCTION IS NOT RE-ENTRANT! It uses
//static local variables
int flash_lpc21xx_write_block(int block_num, unsigned char *buf, int bufsize)
{
	static unsigned int block_start_addr, block_start_index;
	static unsigned int i,j, iap_result_value; 

	if (block_num <0)
	{
		return -EINVAL;
	}else
	{
		block_start_addr = FLASH_LPC21XX_DATA_SECTOR_ADDR + (block_num * FLASH_LPC21XX_BLOCK_SIZE) ; 
		block_start_index = block_start_addr - FLASH_LPC21XX_DATA_SECTOR_ADDR;
		if (block_start_index + bufsize > FLASH_LPC21XX_MAX_BUFFER_SIZE)
		{
			return -EMSGSIZE;
		}
		else
		{
			flash_lpc21xx_init();
			j=0;
			for (i=block_start_index; i<(block_start_index + bufsize);i++)
			{
				sect_value[i] = buf[j];
				j++;
			}
			iap_result_value = flash_lpc21xx_erase_block(0);
			iap_result_value = flash_lpc21xx_program (FLASH_LPC21XX_DATA_SECTOR_ADDR, sect_value, FLASH_LPC21XX_MAX_BUFFER_SIZE); 
		}
	}
	return FLASH_LPC21XX_PROGRAMMING_DONE; 
}


int flash_lpc21xx_erase_block(int block_num)
{
	unsigned int block_start_addr, block_end_addr; 
	if (block_num <0)
	{
		return -EINVAL;
	}
	else
	{
		block_start_addr = (unsigned int) FLASH_LPC21XX_DATA_SECTOR_ADDR + (block_num * FLASH_LPC21XX_BLOCK_SIZE) ; 
		block_end_addr = (unsigned int) block_start_addr + FLASH_LPC21XX_BLOCK_SIZE; 
		flash_lpc21xx_erase (block_start_addr, block_end_addr); 
	}
	return FLASH_LPC21XX_ERASE_DONE; 
}


int flash_lpc21xx_read_block(int block_num, unsigned char *buf, int bufsize)
{
	unsigned int block_start_addr, block_start_index;
	unsigned int i,j; 

	if (block_num <0)
	{
		return -EINVAL;
	}
	else
	{
		block_start_addr = FLASH_LPC21XX_DATA_SECTOR_ADDR + (block_num * FLASH_LPC21XX_BLOCK_SIZE) ; 
		block_start_index = block_start_addr - FLASH_LPC21XX_DATA_SECTOR_ADDR;
		if (block_start_index + bufsize > FLASH_LPC21XX_MAX_BUFFER_SIZE) 
		{
			 return -EMSGSIZE; 
		}
		else
		{
			j = 0; 
			for (i=block_start_index; i<(block_start_index + bufsize);i++)
			{
				buf[j] = sect_value[i];
				j = j+1;
			}
		}
	}
	return FLASH_LPC21XX_READ_DONE; 
}

//Convert 'addr' to Sector Number - From Keil Example prgrm
unsigned int flash_lpc21xx_get_secnum (unsigned int adr)
{
	unsigned int n;

	n = (adr >> 12) & 0x7F;                      // Pseudo Sector Number
	if (n >= 0x78)
	{                             // High Small 4kB Sectors
		n -= 0x62;
	}
  	else if (n >= 0x08)
  	{                        // Large 32kB Sectors
		n  = 0x07 + (n >> 3);
	}

	return (n);                                  // Sector Number
}


//Erase Sector between 'start' and 'end'
//Return: IAP Error Code (0 when OK)
//Note: THIS FUNCTION IS NOT RE-ENTRANT! It uses
//static local variables
unsigned int flash_lpc21xx_erase (unsigned int start, unsigned int end)
{
	static struct iap_in iap;                       // IAP Input Parameters
	static unsigned int  result[16];                // IAP Results
	static unsigned int  save_VicInt;               // for saving VICIntEnable

	save_VicInt = VICIntEnable;              // Save Interrupt Enable Status
	VICIntEnClr = 0xFFFFFFFF;                // Disable all Interrupts

	iap.cmd = 50;                            // IAP Command: Prepare Sectors for Write
	iap.par[0] = flash_lpc21xx_get_secnum (start);         // Start Sector
	iap.par[1] = flash_lpc21xx_get_secnum (end);           // End Sector
	iap_entry (&iap, result);                // Call IAP Function
	if (result[0]) goto exit;                // Error occured?

	iap.cmd = 52;                            // IAP Command: Erase Flash
	iap.par[0] = flash_lpc21xx_get_secnum (start);         // Start Sector
	iap.par[1] = flash_lpc21xx_get_secnum (end);           // End Sector
	iap.par[2] = CCLK;                       // CPU Clock
	iap_entry (&iap, result);                // Call IAP Function

exit:


	VICIntEnable = save_VicInt;              // Restore Interrupt Enable Status

	return (result[0]);
}


/*
  Program *data to addr, number of bytes specified by size
  Return: IAP Error Code (0 when OK)
  NOTES:  size should be 512, 1024, 4096 or 8192
 */
//Note: THIS FUNCTION IS NOT RE-ENTRANT! It uses
//static local variables 
unsigned int flash_lpc21xx_program (unsigned int addr, unsigned char *data, unsigned int size)
{
	static struct iap_in iap;                       // IAP Input Parameters
	static unsigned int  result[16];                // IAP Results
	static unsigned int  save_VicInt;               // for saving VICIntEnable

	save_VicInt = VICIntEnable;              // Save Interrupt Enable Status
	VICIntEnClr = 0xFFFFFFFF;                // Disable all Interrupts

	iap.cmd = 50;                            // IAP Command: Prepare Sectors for Write
	iap.par[0] = flash_lpc21xx_get_secnum (addr);         // Start Sector
	iap.par[1] = iap.par[0];                 // End Sector
	iap_entry (&iap, result);                // Call IAP Function
	if (result[0]) goto exit;                // Error occured?

	iap.cmd = 51;                            // IAP Command: Copy RAM to Flash
	iap.par[0] = addr;                       // Destination Address
	iap.par[1] = (unsigned int) data;        // Source Address
	iap.par[2] = size;                       // Number of Bytes
	iap.par[3] = CCLK;                       // CPU Clock
	iap_entry (&iap, result);                // Call IAP Function

exit:

	VICIntEnable = save_VicInt;              // Restore Interrupt Enable Status

	return (result[0]);
}

